
//
// Created by cc on 2020/7/22.
//

#include <ros/ros.h>
#include "matrix/math.hpp"
#include <cstdlib>
#include <ctime>

using namespace std;
using namespace matrix;

/** Global variables **/
Vector3f _rates_sp, _torque_sp, rates;
float _thrust_sp;

/** Parameters related to multicopter itself **/
Matrix3f inertia_j;
float rotor_base;
float max_motor_drag_force;
float torque_to_drag_coeff;
float mc_weight;

float rotor_base_half;
float rotor_base_half_reciprocal;
float max_pitch_roll_torque;
float max_yaw_torque;
float max_thrust;
float torque_to_drag_coeff_reciprocal;
float max_motor_drag_force_reciprocal;
float max_pitch_roll_torque_reciprocal;
float max_yaw_torque_reciprocal;
float max_thrust_reciprocal;

/**PID parameters**/
Vector3f rates_p_scaled(0.04, 0.04, 0.02);
Vector3f rates_ff(0.001, 0.001, 0.0);

/** Functions **/
float randf(float min, float max, float resolution){
    int num = (max-min)/resolution;
    int result_int = rand() % num;
    float result_float = (float)result_int*resolution + min;
    return result_float;
}

float constrain(float value, float min, float max){
    if(value < min){
        value = min;
    }else if(value > max){
        value = max;
    }else{};

    return value;
}


/*** Parameters for px4 ***/
float 	_tmp_array[4];

float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, float min_output, float max_output)
{
    float k_min = 0.f;
    float k_max = 0.f;

    for (unsigned i = 0; i < 4; i++) {
        // Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway
        if (fabsf(desaturation_vector[i]) < FLT_EPSILON) {
            continue;
        }

        if (outputs[i] < min_output) {
            float k = (min_output - outputs[i]) / desaturation_vector[i];

            if (k < k_min) { k_min = k; }

            if (k > k_max) { k_max = k; }

        }

        if (outputs[i] > max_output) {
            float k = (max_output - outputs[i]) / desaturation_vector[i];

            if (k < k_min) { k_min = k; }

            if (k > k_max) { k_max = k; }
        }
    }

    // Reduce the saturation as much as possible
    return k_min + k_max;
}

void minimize_saturation(const float *desaturation_vector, float *outputs,
                                          float min_output=0.f, float max_output=1.f, bool reduce_only=true)
{
    float k1 = compute_desaturation_gain(desaturation_vector, outputs,  min_output, max_output);

    if (reduce_only && k1 > 0.f) {
        return;
    }

    for (unsigned i = 0; i < 4; i++) {
        outputs[i] += k1 * desaturation_vector[i];
    }

    // Compute the desaturation gain again based on the updated outputs.
    // In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output.
    // In that case adding 0.5 of the gain will equilibrate saturations.
    float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, min_output, max_output);

    for (unsigned i = 0; i < 4; i++) {
        outputs[i] += k2 * desaturation_vector[i];
    }
}

float _rotors[4][4] = {
        { -0.707107,  0.707107,  1.000000,  1.000000 },
        {  0.707107, -0.707107,  1.000000,  1.000000 },
        {  0.707107,  0.707107, -1.000000,  1.000000 },
        { -0.707107, -0.707107, -1.000000,  1.000000 },
};


void mix_yaw(float yaw, float *outputs)
{
    // Add yaw to outputs
    for (unsigned i = 0; i < 4; i++) {
        outputs[i] += yaw * _rotors[i][2];

//		 Yaw will be used to unsaturate if needed
        _tmp_array[i] = _rotors[i][2];
    }

    // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
    // and allow some yaw response at maximum thrust
    minimize_saturation(_tmp_array, outputs, 0.f, 1.15f);

    for (unsigned i = 0; i < 4; i++) {
        _tmp_array[i] = _rotors[i][3];
    }

    // reduce thrust only
    minimize_saturation(_tmp_array, outputs, 0.f, 1.f, true);
}

void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs)
{
    // Airmode for roll and pitch, but not yaw
    // Mix without yaw
    for (unsigned i = 0; i < 4; i++) {
        outputs[i] = roll * _rotors[i][0] +
                     pitch * _rotors[i][1] +
                     thrust * _rotors[i][3];

//		 Thrust will be used to unsaturate if needed
        _tmp_array[i] = _rotors[i][3];
    }

    minimize_saturation(_tmp_array, outputs);

    // Mix yaw independently
    mix_yaw(yaw, outputs);
}

/**************************/


int main(int argc, char** argv){
//    ros::init(argc, argv, "mixer_test");
//    ros::NodeHandle nh;

    srand((int)time(0));

    /**Set MC parameters**/
    rotor_base = 0.21;  //m
    rotor_base_half = rotor_base * 0.5;
    rotor_base_half_reciprocal = 1.f / rotor_base_half;

    max_motor_drag_force = 12.0;  //N
    max_motor_drag_force_reciprocal = 1.f / max_motor_drag_force;

    torque_to_drag_coeff = 0.015;
    torque_to_drag_coeff_reciprocal = 1.f / torque_to_drag_coeff;

    mc_weight = 1.0;  //kg

    max_pitch_roll_torque = 0.707 * rotor_base_half * max_motor_drag_force;
    max_pitch_roll_torque_reciprocal = 1.f / max_pitch_roll_torque;

    max_yaw_torque = max_motor_drag_force * torque_to_drag_coeff * 2; ///Should be *4. Cut a half here.
    max_yaw_torque_reciprocal = 1.f / max_yaw_torque;

    max_thrust = max_motor_drag_force * 4;
    max_thrust_reciprocal = 1.f / max_thrust;

    cout<<"max_pitch_roll_torque="<<max_pitch_roll_torque <<"Nm, max_yaw_torque="<<max_yaw_torque<<"Nm"<<endl;

    float j_matrix[3][3] = {0.1, 0.0, 0.0,
                            0.0, 0.1, 0.0,
                            0.0, 0.0, 0.2}; //kg*m^2
    inertia_j = j_matrix;
    cout<<"Ixx, Iyy, Izz=("<< inertia_j(0,0) << ","<<inertia_j(1,1)<<","<<inertia_j(2,2)<<")"<<endl;

    /** Simulation loop starts **/
    char keyboard_c = '\n';
    long int calculate_times = 0;
    long int warning_times = 0;
    long int error_times = 0;

    clock_t startTime,endTime;
    startTime = clock();

    int operation_times = 10;

    //while (keyboard_c == '\n'){
    for(int times=0; times < operation_times; times++){
        calculate_times ++;
//        cout << "*************Seq=" << calculate_times << endl;

        _rates_sp(0) = randf(-5, 5, 0.001);
        _rates_sp(1) = randf(-5, 5, 0.001);
        _rates_sp(2) = 0.f; // randf(-1, 1, 0.001);
//        cout<<"_rates_sp=("<< _rates_sp(0) << ","<<_rates_sp(1)<<","<<_rates_sp(2)<<")"<<endl;

        _thrust_sp = randf(0.2, 0.8, 0.001);  //Thrust is described by a float ranging from [0,1]
//        cout<<"_thrust_sp="<<_thrust_sp<<endl;

//        rates(0) = randf(-3, 3, 0.001);
//        rates(1) = randf(-3, 3, 0.001);
//        rates(2) = randf(-0.5, 0.5, 0.001);

        rates(0) = 0.f;
        rates(1) = 0.f;
        rates(2) = 0.f;
//        cout<<"rates=("<< rates(0) << ","<<rates(1)<<","<<rates(2)<<")"<<endl;

        /*** Calculate torque setpoint. In mc_att_control_main.cpp.***/
        Vector3f rates_err = _rates_sp - rates;
//        cout<<"rates_err=("<< rates_err(0) << ","<<rates_err(1)<<","<<rates_err(2)<<")"<<endl;

        Vector3f rates_to_torque_pid_result = rates_p_scaled.emult(rates_err); ///Should be PID in PX4. Simplify to P here.
//        cout<<"rates_to_torque_pid_result=("<< rates_to_torque_pid_result(0) << ","<<rates_to_torque_pid_result(1)<<","<<rates_to_torque_pid_result(2)<<")"<<endl;

        _torque_sp = rates.cross(inertia_j*rates) + rates_ff.emult(inertia_j*rates_err) + rates_to_torque_pid_result;

        cout<<"_torque_sp=("<< _torque_sp(0) << ","<<_torque_sp(1)<<","<<_torque_sp(2)<<")"<<endl;
        cout <<"_thrust_sp="<<_thrust_sp<<endl;

        /// Map to values used in PX4 considering maximum value
        _torque_sp(0) = _torque_sp(0) * max_pitch_roll_torque_reciprocal;
        _torque_sp(1) = _torque_sp(1) * max_pitch_roll_torque_reciprocal;
        _torque_sp(2) = _torque_sp(2) * max_yaw_torque_reciprocal;
        /// Set limitation. (Can't reach the constrains at all with this simulated quadrotor.)
        _torque_sp(0) = constrain(_torque_sp(0), -1, 1);
        _torque_sp(1) = constrain(_torque_sp(1), -1, 1);
        _torque_sp(2) = constrain(_torque_sp(2), -1, 1);
//        cout<<"_torque_sp_mapped=("<< _torque_sp(0) << ","<<_torque_sp(1)<<","<<_torque_sp(2)<<")"<<endl;


        /*** Mixer. In mixer_multirotor.cpp ***/
        /// Map back to real torque and thrust value

        float outputs[4];
        mix_airmode_rp(_torque_sp(0), _torque_sp(1), _torque_sp(2), _thrust_sp, outputs);

        /*** Now Simulate torque and force output ***/
        float force_px4[4];
        cout << "px4 mixer: (";
        for(int i=0; i<4; i++){
            cout << outputs[i] << ", ";
            outputs[i] = constrain(-1.f +2*outputs[i], -1.f, 1.f);
            force_px4[i] = (outputs[i] + 1.f) / 2.f * max_motor_drag_force;
        }
        cout << ")"<< endl;

        Vector3f out_torque_px4;
        float inter = 0.707 * rotor_base_half;
        out_torque_px4(0) = -inter * force_px4[0] + inter*force_px4[1] + inter*force_px4[2] - inter*force_px4[3];
        out_torque_px4(1) = inter * force_px4[0] - inter*force_px4[1] + inter*force_px4[2] - inter*force_px4[3];
        out_torque_px4(2) = torque_to_drag_coeff * (force_px4[0] + force_px4[2] - force_px4[2] - force_px4[3]);
        float out_thrust_px4 = force_px4[0] + force_px4[1] + force_px4[2] + force_px4[3];

        cout << "out_torque_px4=(" << out_torque_px4(0) <<", "<<out_torque_px4(1)<<", "<<out_torque_px4(2)<<")"<<endl;
        cout << "out_thrust_px4=" << out_thrust_px4 <<endl;

        /**my mixer*/
        Vector3f _torque_sp_real;
        _torque_sp_real(0) = _torque_sp(0) * max_pitch_roll_torque;
        _torque_sp_real(1) = _torque_sp(1) * max_pitch_roll_torque;
        _torque_sp_real(2) = _torque_sp(2) * max_yaw_torque;
        float _thrust_sp_real = _thrust_sp * max_thrust;

        float _a = _thrust_sp_real * 0.5f;
        float _b = _torque_sp_real(0) * 0.707 * rotor_base_half_reciprocal;
        float _d = _torque_sp_real(1) * 0.707 * rotor_base_half_reciprocal;

        /// Calculate motor drag considering pitch, roll and thrust control.
        float f1=0.f, f2=0.f, f3=0.f, f4=0.f;
        for(int step=0; step<=10; step++){  // Calculate f4
            float cond_min[4], cond_max[4];
            cond_min[0] = _a - _b - max_motor_drag_force;
            cond_max[0] = _a - _b;
            cond_min[1] = _a - _d - max_motor_drag_force;
            cond_max[1] = _a - _d;
            cond_min[2] = -_b - _d;
            cond_max[2] = -_b - _d + max_motor_drag_force;
            cond_min[3] = 0;
            cond_max[3] = max_motor_drag_force;

            float f4_min = -1000.f;
            float f4_max = 1000.f;
            for(int i=0; i<4; i++){
                if(cond_min[i] > f4_min){  //find max among cond_min[i]
                    f4_min = cond_min[i];
                }
                if(cond_max[i] < f4_max){  //find min among cond_max[i]
                    f4_max = cond_max[i];
                }
            }

            if(f4_min <= f4_max){
                f4 = (f4_min + f4_max) * 0.5f;
                break;
            }else{
                cout << "WARNING: motor saturation occurs!" <<endl;
                if(cond_min[0] > f4_max || cond_min[1] > f4_max){
                    _a = _a * 0.95f;
                }else if(cond_max[0] < f4_min || cond_max[1] < f4_min){
                    _a = _a * 1.05f;
                }else{
                    cout << "Error!!!!!! This should not happen!!" <<std::endl;
                    f4 = (f4_min + f4_max) / 2.f;
                    error_times ++;
                    break;
                }
            }
        }
//        cout<<"_a is corrected from " << _thrust_sp_real * 0.5f << " to " << _a << endl;

        f1 = _a - _b - f4;
        f2 = _a - _d - f4;
        f3 = _b + _d + f4;

        if(_thrust_sp_real < 0.1f){
                f1= f2 = f3 = f4 = 0.f;
        }

        /// Now consider yaw control
        float delt_fi = _torque_sp_real(2) * 0.25 * torque_to_drag_coeff_reciprocal;
        float delt_fi_max = 1000.f;
        if(delt_fi >= 0){
            max_motor_drag_force-f1 < delt_fi_max ? delt_fi_max= max_motor_drag_force-f1 : true;
            max_motor_drag_force-f2 < delt_fi_max ? delt_fi_max= max_motor_drag_force-f2 : true;
            f3 < delt_fi_max ? delt_fi_max= f3 : true;
            f4 < delt_fi_max ? delt_fi_max= f4 : true;
        }else{
            f1 < delt_fi_max ? delt_fi_max=f1 : true;
            f2 < delt_fi_max ? delt_fi_max=f2 : true;
            max_motor_drag_force-f3 < delt_fi_max ? delt_fi_max= max_motor_drag_force-f3 : true;
            max_motor_drag_force-f4 < delt_fi_max ? delt_fi_max= max_motor_drag_force-f4 : true;
        }

        if(delt_fi > delt_fi_max) {
            delt_fi = delt_fi_max;
//            cout << "$$ Yaw saturation accurs!" << endl;
        }else if(delt_fi < -delt_fi_max){
            delt_fi = -delt_fi_max;
        }

        f1 += delt_fi;
        f2 += delt_fi;
        f3 -= delt_fi;
        f4 -= delt_fi;

        /// Map to [0, 1]
        float out_put[4];
        out_put[0] = f1 * max_motor_drag_force_reciprocal;
        out_put[1] = f2 * max_motor_drag_force_reciprocal;
        out_put[2] = f3 * max_motor_drag_force_reciprocal;
        out_put[3] = f4 * max_motor_drag_force_reciprocal;

        for(int i=0; i<4; i++){
            out_put[i] = constrain(out_put[i], 0, 1);
        }

//        cout << "f=("<<f1<<", "<<f2<<", "<<f3<<", "<<f4<<")" <<endl;
        cout << "my_mixer=("<<out_put[0]<<", "<<out_put[1]<<", "<<out_put[2]<<", "<<out_put[3]<<")" <<endl;

        //cout<<"Press enter to continue. or others + enter to quit" << endl;
        //keyboard_c = getchar();

        /*** Now Simulate torque and force output ***/
        float force[4];
        for(int i=0; i<4; i++){
            out_put[i] = constrain(-1.f +2*out_put[i], -1.f, 1.f);
            force[i] = (out_put[i] + 1.f) / 2.f * max_motor_drag_force;
        }

        Vector3f out_torque;
        out_torque(0) = -inter * force[0] + inter*force[1] + inter*force[2] - inter*force[3];
        out_torque(1) = inter * force[0] - inter*force[1] + inter*force[2] - inter*force[3];
        out_torque(2) = torque_to_drag_coeff * (force[0] + force[2] - force[2] - force[3]);
        float out_thrust = force[0] + force[1] + force[2] + force[3];

        cout << "out_torque=(" << out_torque(0) <<", "<<out_torque(1)<<", "<<out_torque(2)<<")"<<endl;
        cout << "out_thrust=" << out_thrust <<endl << endl;

    }
    endTime = clock();
    cout << endl << endl;
    double time = (double)(endTime - startTime) / CLOCKS_PER_SEC;
    cout << "The run time is: " << time << "s" << endl;
    cout << "The run time for each loop is: " << time / (double)operation_times * 1000000 << "us" << endl;

    cout << "Error times = " << error_times << endl;
    cout << "Warning times = " << warning_times <<endl;

    return 0;
}